#include "stepping_motor_driver.h"

static float motor_speed_x = 0, motor_speed_y = 0, motor_speed_w = 0;

speedRampData srd= {STOP,0,0,0,0,0,0}; //�Ӽ��ٱ���    

uint8_t  motor_sta        = 0;//���״�?

TaskHandle_t speed_mix_handler;
TaskHandle_t speed_x_handler;
TaskHandle_t speed_y_handler;
TaskHandle_t speed_w_handler;

//----------����ԭ�ӵ���Ӽ������?-----------------
void stepping_motor_init()
{
    GPIO_InitTypeDef  GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
	TIM_OCInitTypeDef TIM_OCInitStructure;
	NVIC_InitTypeDef  NVIC_InitStructure;

	//GPIO����
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE,ENABLE); 	
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;   //����
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
	GPIO_Init(GPIOE, &GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3;  //STP
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
	GPIO_Init(GPIOA, &GPIO_InitStructure);
		
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource0, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource1, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_TIM2); 

	//��ʱ������
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
	
	TIM_InternalClockConfig(TIM2);
	
	TIM_TimeBaseStructure.TIM_Prescaler = TIM_PRESCALER;
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseStructure.TIM_Period = 65536-1;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	
	TIM_OCStructInit(&TIM_OCInitStructure);	
	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;  						//��תģʽ
	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	TIM_OCInitStructure.TIM_Pulse = 0;  	//CCR �Ƚ�ֵ
	
	TIM_OC1Init(TIM2, &TIM_OCInitStructure);
	TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);

	TIM_OC2Init(TIM2, &TIM_OCInitStructure);
	TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Disable);

	TIM_OC3Init(TIM2, &TIM_OCInitStructure);
	TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Disable);

	TIM_OC4Init(TIM2, &TIM_OCInitStructure);
	TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Disable);
  
	TIM_CCxCmd(TIM2, TIM_Channel_1, TIM_CCx_Disable);
	TIM_CCxCmd(TIM2, TIM_Channel_2, TIM_CCx_Disable);
	TIM_CCxCmd(TIM2, TIM_Channel_3, TIM_CCx_Disable);
	TIM_CCxCmd(TIM2, TIM_Channel_4, TIM_CCx_Disable);
	
	TIM_ARRPreloadConfig(TIM2, ENABLE);
	
	TIM_ClearFlag(TIM2, TIM_FLAG_CC1);		  	                //�����ʱ����ͨ��?1�ı�־λ
	TIM_ClearFlag(TIM2, TIM_FLAG_CC2);
	TIM_ClearFlag(TIM2, TIM_FLAG_CC3);
	TIM_ClearFlag(TIM2, TIM_FLAG_CC4);
	
	TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);														//������ʱ��3��ͨ��1�ж�
  	TIM_ITConfig(TIM2, TIM_IT_CC2, ENABLE);	
	TIM_ITConfig(TIM2, TIM_IT_CC3, ENABLE);	
	TIM_ITConfig(TIM2, TIM_IT_CC4, ENABLE);	
	
	//NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);									//����
	
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_Init(&NVIC_InitStructure);
}

void MOTOR_Move(int32_t step, uint32_t accel, uint32_t decel, uint32_t speed)
{
		
    uint16_t tim_count; 																										 //����ж�ʱ�̵ļ����?
    unsigned int max_s_lim;       //�ﵽ����ٶ�ʱ�Ĳ���?    
    unsigned int accel_lim;		 //���뿪ʼ���ٵĲ����������û�м��ٶȵ�����ٶ�ʱ��

	if(motor_sta!= STOP)  		 //ֻ�������������ֹͣ��ʱ��ż���
		return;			
    if(step == 1)   																											   // ���ֻ�ƶ�һ��?
    {       
      srd.accel_count = -1; 	 // ֻ�ƶ�һ��
        
      srd.run_state = DECEL;	 // ����״̬
        
      srd.step_delay = 1000;	 // ����ʱ

    }
    
    else if(step != 0)  // ������Ϊ����ƶ�?
    {					
		srd.min_delay = (int32_t)(A_T_x10/speed);	// ��������ٶȼ���?, ����min_delay���ڶ�ʱ���ļ�������ֵmin_delay = (alpha / tt)/ w   
		srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10);		// ͨ�������һ��?(c0) �Ĳ�����ʱ���趨���ٶ�,����accel��λΪ0.01rad/sec^2
	// step_delay = 1/tt * sqrt(2*alpha/accel)
	// step_delay = ( tfreq*0.69/10 )*10 * sqrt( (2*alpha*100000) / (accel*10) )/100 
		max_s_lim = (uint32_t)(speed*speed/(A_x200*accel/10));//������ٲ�֮��ﵽ����ٶȵ�����? max_s_lim = speed^2 / (2*alpha*accel)

		if(max_s_lim == 0)	//����ﵽ����ٶ�С��0.5�������ǽ���������Ϊ0,��ʵ�����Ǳ����ƶ�����һ�����ܴﵽ��Ҫ���ٶ� 
		{
			max_s_lim = 1;
		}    
		accel_lim = (uint32_t)(step*decel/(accel+decel)); // ������ٲ�֮�����Ǳ��뿪ʼ����?,n1 = (n1+n2)decel / (accel + decel)

		if(accel_lim == 0) 																										// ���Ǳ����������?1�����ܿ�ʼ����
		{
			accel_lim = 1;
		}

		if(accel_lim <= max_s_lim)		//���ٽ׶ε���������ٶȾ͵ü��١�����ʹ�������������ǿ��Լ�������ٽ׶β��� 
		{
			srd.decel_val = accel_lim - step;		//���ٶεĲ���
		}
		else
		{
			srd.decel_val = -(max_s_lim*accel/decel);	//���ٶεĲ��� 
		}

		if(srd.decel_val == 0) 		// ����һ�� ��һ������ 
		{
			srd.decel_val = -1;
		}    
		srd.decel_start = step + srd.decel_val;		//���㿪ʼ����ʱ�Ĳ���


		if(srd.step_delay <= srd.min_delay)			// ���һ��ʼc0���ٶȱ����ٶ��ٶȻ��󣬾Ͳ���Ҫ���м����˶���ֱ�ӽ�������
		{
			srd.step_delay = srd.min_delay;
			srd.run_state = RUN;
		}
		else
		{
			srd.run_state = ACCEL;
		}

		srd.accel_count = 0;		// ��λ���ٶȼ���ֵ
	}
		motor_sta = 1;  																												// ���Ϊ�˶�״�?
		tim_count = TIM_GetCounter(TIM3);		//��ȡ����ֵ
		TIM_SetCompare1(TIM2,tim_count+srd.step_delay/2);		//���ö�ʱ���Ƚ�ֵ 
		TIM_SetCompare2(TIM2,tim_count+srd.step_delay/2);		
		TIM_SetCompare3(TIM2,tim_count+srd.step_delay/2);		
		TIM_SetCompare4(TIM2,tim_count+srd.step_delay/2);		
		
		TIM_ITConfig(TIM2,TIM_IT_CC1,ENABLE);	//ʹ�ܶ�ʱ��ͨ�� 
		TIM_ITConfig(TIM2,TIM_IT_CC2,ENABLE);	
		TIM_ITConfig(TIM2,TIM_IT_CC3,ENABLE);	
		TIM_ITConfig(TIM2,TIM_IT_CC4,ENABLE);	
		
		TIM_CCxCmd(TIM2,TIM_Channel_1,TIM_CCx_Enable);
		TIM_CCxCmd(TIM2,TIM_Channel_2,TIM_CCx_Enable);
		TIM_CCxCmd(TIM2,TIM_Channel_3,TIM_CCx_Enable);
		TIM_CCxCmd(TIM2,TIM_Channel_4,TIM_CCx_Enable);
		
		TIM_Cmd(TIM2, ENABLE);	//������ʱ��
}

void speed_decision()   //�ж�ִ�к���
{
	__IO uint32_t tim_count=0;
	__IO uint32_t tmp = 0;  
  uint16_t new_step_delay=0;    // �����£��£�һ����ʱ����  
  __IO static uint16_t last_accel_delay=0;   // ���ٹ��������һ����ʱ���������ڣ�?. 
  __IO static uint32_t step_count = 0; 		// ���ƶ�����������  
  __IO static int32_t rest = 0;		// ��¼new_step_delay�е������������һ������ľ���  
  __IO static uint8_t i=0;			//��ʱ��ʹ�÷�תģʽ����Ҫ���������жϲ����һ����������?
 
  if (TIM_GetITStatus(TIM2, TIM_IT_CC1)!=RESET)
  {	  
		
    TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);	// �����ʱ���ж�?		
	TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
	TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
//		TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
		
	tim_count = TIM_GetCounter(TIM2);	//��ȡ����ֵ
		
	tmp = tim_count+srd.step_delay/2;
		
	TIM_SetCompare1(TIM2,tmp);	// ���ñȽ�ֵ
	TIM_SetCompare2(TIM2,tmp);		
	TIM_SetCompare3(TIM2,tmp);		
	TIM_SetCompare4(TIM2,tmp);		
		
	i++; 
	if(i==2)		//�ж�����Ϊһ������
	{
		i=0; 
		switch(srd.run_state)
		{
			case STOP:		//ֹͣ״̬
				step_count = 0;
				rest = 0;
				
				TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
				TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
				TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
				TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
			
				TIM_CCxCmd(TIM2,TIM_Channel_1,TIM_CCx_Disable);
				TIM_CCxCmd(TIM2,TIM_Channel_2,TIM_CCx_Disable);
				TIM_CCxCmd(TIM2,TIM_Channel_3,TIM_CCx_Disable);
				TIM_CCxCmd(TIM2,TIM_Channel_4,TIM_CCx_Disable);
			
				TIM_Cmd(TIM2, DISABLE);	 //����������Թرն�ʱ����������ֻ�ر�ͨ������?
				motor_sta = 0;  
				break;
			
			case ACCEL:	//����״̬
			step_count++;
			srd.accel_count++;
			new_step_delay = srd.step_delay - (((2 *srd.step_delay) + rest)/(4 * srd.accel_count + 1));//������(��)һ����������(ʱ����)
			rest = ((2 * srd.step_delay)+rest)%(4 * srd.accel_count + 1);	// �����������´μ��㲹���������������?
			
				if(step_count >= srd.decel_start) 		//����ǹ�Ӧ�ÿ�ʼ����?
				{
					srd.accel_count = srd.decel_val;	//���ټ���ֵΪ���ٽ׶μ���ֵ�ĳ�ʼֵ
					srd.run_state = DECEL;		//�¸����������ٽ׶� 
				}
				
				else if(new_step_delay <= srd.min_delay)//����Ƿ񵽴�����������ٶ�
				{
					last_accel_delay = new_step_delay;		//������ٹ��������һ����ʱ���������ڣ�
					new_step_delay = srd.min_delay;   		// ʹ��min_delay����Ӧ����ٶ�speed�� 
					rest = 0;            		//������ֵ               
					srd.run_state = RUN;		//����Ϊ��������״̬ 
				}
				break;
				
			case RUN:
          step_count++;  		// ������1				  
          new_step_delay = srd.min_delay;    // ʹ��min_delay����Ӧ����ٶ�speed��				 
          if(step_count >= srd.decel_start)  // ��Ҫ��ʼ����
					{
            srd.accel_count = srd.decel_val;  	// ���ٲ�����Ϊ���ټ���ֵ
            new_step_delay = last_accel_delay;	// �ӽ׶�������ʱ��Ϊ���ٽ׶ε���ʼ��ʱ(��������)
            srd.run_state = DECEL;             // ״̬�ı�Ϊ����
          }
          break;
					
				case DECEL:
          step_count++;  		// ������1

          srd.accel_count++; 	// �Ǹ�����
          new_step_delay = srd.step_delay - (((2 * srd.step_delay) + rest)/(4 * srd.accel_count + 1)); //������(��)һ����������(ʱ����)
          rest = ((2 * srd.step_delay)+rest)%(4 * srd.accel_count + 1);				// �����������´μ��㲹���������������?
          if(srd.accel_count >= 0) 	//����Ƿ�Ϊ���һ��  �Ǹ���������Ҫ�ж� ���ڵ�����ʱ Ӧ�þ��Ǽ��ٽ���
          {
            srd.run_state = STOP;
          }
          break;
        }
			 srd.step_delay = new_step_delay; 	// Ϊ�¸�(�µ�)��ʱ(��������)��ֵ
		}
	}
}

speedRampData* get_motor_sta()
{
	return &srd;
}

void TIM2_IRQHandler(void)
{
	speed_decision();
}
//------------------------------------------------

void usart_ctrl_steeping_motoe_Init()
{
	usart3_init(115200);									//��̨������ƴ���?
	uart4_init(115200);										//���̵�����ƴ���?
}

//��̨������ƺ���?
void usart_ctrl_gimbal_motor(steeping_motor motor)
{
	char ctrl_location_arr[13] = {	motor.motor_ID, 				//��ַ
									0xfd, 							//ָ����
									motor.motor_direction, 			//����
									motor.motor_speed >> 8, 		//�ٶȸ�8λ
									motor.motor_speed, 				//�ٶȵ�8λ
									motor.motor_acc_speed,			//���ٶȵ�λ
									motor.motor_pulse_num >> 24,
									motor.motor_pulse_num >> 16, 	//��������16λ
									motor.motor_pulse_num >> 8,
									motor.motor_pulse_num , 		//��������16λ
									0x00,							//��ԣ�����λ��?
									motor.synchronous,				//���ͬ��?
									0x6b};							//У��

	for(char i = 0; i < 13; i++)
	{
		while((USART3->SR&0X40)==0);
		USART3->DR = ctrl_location_arr[i];    
	}
	delay_xms(1);
}

//��̨���ͬ������?
void usart_ctrl_gimbal_motor_synchronous()
{
	char synchronous_move[4] = {0x00, 0xff, 0x66, 0x6b};
	
	for(char i = 0; i < 4; i++)
	{
		while((USART3->SR&0X40)==0);
		USART3->DR = synchronous_move[i];    
	}
	delay_xms(5);
}

//���̵�����ƺ���?
void usart_ctrl_chassis_motor(steeping_motor motor, char speed_or_distence)
{
	if(speed_or_distence == 0)	//����ģʽ
	{
		char ctrl_location_arr[13] = {	motor.motor_ID, 				//��ַ
										0xfd, 							//ָ����
										motor.motor_direction, 			//����
										motor.motor_speed >> 8, 		//�ٶȸ�8λ
										motor.motor_speed, 				//�ٶȵ�8λ
										motor.motor_acc_speed,			//���ٶȵ�λ
										motor.motor_pulse_num >> 24,
										motor.motor_pulse_num >> 16, 	//��������16λ
										motor.motor_pulse_num >> 8,
										motor.motor_pulse_num , 		//��������16λ
										0x00,							//��ԣ�����λ��?
										motor.synchronous,				//���ͬ��?
										0x6b};							//У��

		for(char i = 0; i < 13; i++)
		{
			while((UART4->SR&0X40)==0);
			UART4->DR = ctrl_location_arr[i];
		}
	}
	else if(speed_or_distence == 1)
	{
		char ctrl_speed_arr[8] = {	motor.motor_ID, 					//��ַ
									0xf6, 								//ָ����
									motor.motor_direction, 				//����
									motor.motor_speed >> 8, 			//�ٶȸ�8λ
									motor.motor_speed, 					//�ٶȵ�8λ
									motor.motor_acc_speed, 				//���ٶ�
									motor.synchronous, 					//���ͬ��?
									0x6b};								//У��

		for(char i = 0; i < 8; i++)
		{
			while((UART4->SR&0X40)==0);
			UART4->DR = ctrl_speed_arr[i];
		}
	}
	delay_xms(1);
}

//���̵��ͬ������?
void usart_ctrl_chassis_motor_synchronous()
{
	char synchronous_move[4] = {0x00, 0xff, 0x66, 0x6b};
	
	for(char i = 0; i < 4; i++)
	{
		while((UART4->SR&0X40)==0);
		UART4->DR = synchronous_move[i];    
	}

	delay_xms(1);
}

//���û���λ��ID1���û���
void set_gimbal_motor_zero(steeping_motor motor)
{
	char set_zero[5] = {motor.motor_ID, 0x93, 0x88, 0x01, 0x6b};//���û���ָ��			

	for(char i = 0; i < 5; i++)
	{
		while((USART3->SR&0X40)==0);
		USART3->DR = set_zero[i];    
	}	
}

//��������
void gimbal_motor_return_zero(steeping_motor motor)
{
	char set_zero[5] = {motor.motor_ID, 0x9a, 0x00, 0x00, 0x6b};//��������ָ��	

	for(char i = 0; i < 5; i++)
	{
		while((USART3->SR&0X40)==0);
		USART3->DR = set_zero[i];    
	}
	delay_xms(1);
}

//---------��RTOS�ķ������?-------------

//x�����ٶȿ���
void speed_x()
{
    while(1)
    {
		//if(motor_speed_x != 0) motor_speed_x += x_correct;//��������

        xQueueSend( speed_x_queue, 	/* ��Ϣ���еľ��? */
                    &motor_speed_x,	/* ���͵���Ϣ���� */
                    0 );        	/* �ȴ�ʱ�� 0 */         

        delay_xms(10);   
    }
}

//y�����ٶȿ���
void speed_y()
{
    while(1)
    {
		//if(motor_speed_y != 0) motor_speed_y += y_correct;//��������

        xQueueSend( speed_y_queue, 	/* ��Ϣ���еľ��? */
                    &motor_speed_y,	/* ���͵���Ϣ���� */
                    0 );        	/* �ȴ�ʱ�� 0 */       

        vTaskDelay(10);   
    }
}

//ת���ٶȿ���
void speed_w()
{
    while(1)
    {
		//if(motor_speed_w != 0) motor_speed_w += w_correct;//��������

        xQueueSend( speed_w_queue, 	/* ��Ϣ���еľ��? */
                    &motor_speed_w,	/* ���͵���Ϣ���� */
                    0 );        	/* �ȴ�ʱ�� 0 */       
        vTaskDelay(10);

		// motor_speed_w = 90;
		// vTaskDelay(1070);
    }
}

//�ٶȻ�Ͽ�������?
//�ֱ���Ϊ scan_time ms
void speed_mix()
{
    static float rec_speed_x_queue_buff = 0, rec_speed_y_queue_buff = 0, rec_speed_w_queue_buff = 0;

	steeping_motor MOTOR_1, MOTOR_2, MOTOR_3, MOTOR_4;

	MOTOR_1.motor_ID = 1;
	MOTOR_2.motor_ID = 2;
	MOTOR_3.motor_ID = 3;
	MOTOR_4.motor_ID = 4;

	//Ĭ�ϵ�����ٶ�?
	MOTOR_1.motor_acc_speed = 200;
	MOTOR_2.motor_acc_speed = 200;
	MOTOR_3.motor_acc_speed = 200;
	MOTOR_4.motor_acc_speed = 200;

	//���ֱ�з���?
	MOTOR_1.motor_direction = 1;
	MOTOR_2.motor_direction = 0;
	MOTOR_3.motor_direction = 1;
	MOTOR_4.motor_direction = 0;

	MOTOR_1.synchronous = MOTOR_2.synchronous =  MOTOR_3.synchronous = MOTOR_4.synchronous = 0;

    while(1)
    {
        xQueueReceive(speed_x_queue, &rec_speed_x_queue_buff, 0);
        xQueueReceive(speed_y_queue, &rec_speed_y_queue_buff, 0);
        xQueueReceive(speed_w_queue, &rec_speed_w_queue_buff, 0);

		//printf("%f %f %f\r\n", rec_speed_x_queue_buff, rec_speed_y_queue_buff, rec_speed_w_queue_buff);

		//�ٶ��ں�
		int motor_1_speed = (int)rec_speed_x_queue_buff - (int)rec_speed_y_queue_buff - (int)rec_speed_w_queue_buff;
		int motor_2_speed = (int)rec_speed_x_queue_buff + (int)rec_speed_y_queue_buff + (int)rec_speed_w_queue_buff;
		int motor_3_speed = (int)rec_speed_x_queue_buff + (int)rec_speed_y_queue_buff - (int)rec_speed_w_queue_buff;
		int motor_4_speed = (int)rec_speed_x_queue_buff - (int)rec_speed_y_queue_buff + (int)rec_speed_w_queue_buff;

		//����������?
		MOTOR_1.motor_direction = MOTOR_3.motor_direction = 1;
		MOTOR_2.motor_direction = MOTOR_4.motor_direction = 0;

		if(motor_1_speed < 0)
		{
			MOTOR_1.motor_direction = 0;
			motor_1_speed = motor_1_speed * -1;
		}

		if(motor_2_speed < 0) 
		{
			MOTOR_2.motor_direction = 1;
			motor_2_speed = motor_2_speed * -1;
		}

		if(motor_3_speed < 0) 
		{
			MOTOR_3.motor_direction = 0;
			motor_3_speed = motor_3_speed * -1;
		}

		if(motor_4_speed < 0) 
		{
			MOTOR_4.motor_direction = 1;
			motor_4_speed = motor_4_speed * -1;
		}

		MOTOR_1.motor_speed = motor_1_speed;
		MOTOR_2.motor_speed = motor_2_speed;
		MOTOR_3.motor_speed = motor_3_speed;
		MOTOR_4.motor_speed = motor_4_speed;

		//д������
		usart_ctrl_chassis_motor(MOTOR_1, 1);
		usart_ctrl_chassis_motor(MOTOR_2, 1);
		usart_ctrl_chassis_motor(MOTOR_3, 1);
		usart_ctrl_chassis_motor(MOTOR_4, 1);
		usart_ctrl_chassis_motor_synchronous();

        vTaskDelay(scan_time);
    }
}

void change_xyw_speed(float x, float y, float w)
{
	motor_speed_x = x;
	motor_speed_y = y;
	motor_speed_w = w;
}

//---------------------------------------------


//------------����RTOS�ķ������?----------------
static steeping_motor MOTOR[4];

void motor_soft_Init()
{
	for(char i = 0;i < 4; i++) 
	{
		MOTOR[i].motor_ID = i + 1;			//���ID
		MOTOR[i].motor_acc_speed = 200;		//Ĭ�ϵ�����ٶ�?
		MOTOR[i].synchronous = 0;
	}

	//���ֱ�з���?
	MOTOR[0].motor_direction = 1;
	MOTOR[1].motor_direction = 0;
	MOTOR[2].motor_direction = 1;
	MOTOR[3].motor_direction = 0;
}

//�����ٶ�
void change_xyw_speed_no_quene(int x, int y, int w)
{
	//�ٶ��ں�
	int motor_out_speed[4];

	motor_out_speed[0] = x - y - w;
	motor_out_speed[1] = x + y + w;
	motor_out_speed[2] = x + y - w;
	motor_out_speed[3] = x - y + w;

	//����������?
	MOTOR[0].motor_direction = MOTOR[2].motor_direction = 1;
	MOTOR[1].motor_direction = MOTOR[3].motor_direction = 0;

	for(char i = 0; i<4; i++) 
	{
		if(motor_out_speed[i] < 0)//����δ�����ı䷽����ٶȱ�Ϊ����?
		{
			MOTOR[i].motor_direction = !MOTOR[i].motor_direction;
			motor_out_speed[i] = motor_out_speed[i] * -1;
		}
	}

	//д��
	for(char i = 0; i<4; i++) 
	{
		MOTOR[i].motor_speed = motor_out_speed[i];
		usart_ctrl_chassis_motor(MOTOR[i], 1);
	}

	usart_ctrl_chassis_motor_synchronous();
}

void read_motor_voltage()
{
	char read_volatge[4] = {0x01, 0x24, 0x6b};
	
	for(char i = 0; i < 3; i++)
	{
		while((USART3->SR&0X40)==0);
		USART3->DR = read_volatge[i];
		//printf("%c", read_volatge[i]);
	}
	
	delay_xms(1);
}

//--------------------------------------------